/* clock em Hz */
#define F_CPU 16000000UL

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

volatile uint16_t H_COUNTER;
volatile uint8_t on;

volatile int SERVO_POS;

ISR(TIMER2_OVF_vect) {

  H_COUNTER++;
  
  if (H_COUNTER >= SERVO_POS){
    PORTC &= ~(1<<PC5);
    on = 0;
  }
  if ((H_COUNTER >= 125)&&(on)){
    PORTC &= ~(1<<PC5);
    on = 0;
  } 
  if (H_COUNTER >= 1250){
    PORTC |= 1 << PC5;
    on = 1;
    H_COUNTER = 0;
  }
}

void setup_timer2(void){
  /* No pre-scaler (16Mhz) */
  TCCR2B |= _BV(CS20);
  /* Enable Timer Overflow Interrupts */
  TIMSK2 |= _BV(TOIE2);
  /* Reset Counters */
  TCNT2 = 0;
  H_COUNTER = 0;
}

int main(void) {

  setup_timer2();
  /* other set up */
  DDRC |= 1 << PC5;
  PORTC |= 1 << PC5;
  on = 1;
  SERVO_POS = 93;

  /* Enable Interrupts */
  sei();
  
  while (1){
    _delay_ms(1000);
    if (SERVO_POS>90){
      SERVO_POS = 62;
    } else {
      SERVO_POS = 124;
    }
  }
};  /* empty loop */

